#include <MS561101BA.h>
#include <Wire.h>


/***************************************************************************************/
/* sensitivity / data filtering / Kalman filter parameters                             */
/* The Kalman Filter is being used to remove noise from the MS5611 sensor data. It can */
/* be adjusted by changing the value Q (process Noise) and R (Sensor Noise).           */
/* Best pactise is to only change R, as both parametrers influence each other.         */
/* (less Q requires more R to compensate and vica versa)                               */
/***************************************************************************************/
#define KALMAN_Q 0.05 // 0.05 do not tamper with this value unless you are looking for adventure ;-)
#define KALMAN_R 1000  // default:300   change this value if you want to adjust the default sensitivity!
                      // this will only be used if PIN_PPM is NOT defined
                      //  50 = fast but lot of errors (good for sensor testing...but quite a lot of noise)
                      // 300 = medium speed and less false indications (nice medium setting with 
                      //       a silence window (indoor)of -0.2 to +0.2)
                      // 1000 = conservative setting ;-)
                      // 500 = still useable maybe for a slow flyer?
                      // 1000 and q=0.001 is it moving at all?? in stable airpressure you can measure 
                      //      the height of your furniture with this setting. but do not use it for flying ;-)
                      // .. everything inbetween in relation to these values.


//#define OSS		MS561101BA_OSR_256
#define OSS		MS561101BA_OSR_512
//#define OSS		MS561101BA_OSR_1024
//#define OSS		MS561101BA_OSR_2048
//#define OSS		MS561101BA_OSR_4096

MS561101BA Baro = MS561101BA();


double kalman_q = KALMAN_Q; //process noise covariance
double kalman_r = KALMAN_R; //measurement noise covariance

double rawAlt = 0, lastAlt = 0;
long clibmRate = 0;

const int pressureCount = 6;
double savedPressure[pressureCount];
double avgPressure;

const int climbRateQueueCount = 10;
double climbRateQueue[climbRateQueueCount];

unsigned long lastMillis;

void saveClimbRate(double altitude)
{
	static double lastAltitude = altitude;
	static long lastMicros = micros();
	static int count = 0;

	long now = micros();
	double climbRate = (double)(altitude - lastAltitude) * ((double)1000000 / (double)(now - lastMicros));

	climbRateQueue[count] = climbRate;
	count = (count + 1) % climbRateQueueCount;

	lastMicros = now;
	lastAltitude = altitude;
}

double getClimbRate()
{
	double climbRate = 0;
	
	for( int i = 0; i < climbRateQueueCount; i++ )
		climbRate += climbRateQueue[i];

	return climbRate / climbRateQueueCount;
}

void savePressure(double pressure)
{
	static int count = 0;

	savedPressure[count] = pressure;
	count = (count + 1) % pressureCount;
}

double getAveragePressure()
{
	double sum = 0;
	
	for( int i = 0; i < pressureCount; i++ )
		sum += savedPressure[i];
		
	return sum / pressureCount;
}

const double def_sea_press = 1013.25;

/*
double getAltitude(double sea_press)
{
  double temp = Baro.getTemperature(OSS);
  double press = Baro.getPressure(OSS);

  return ((pow((sea_press / press), 1/5.257) - 1.0) * (temp + 273.15)) / 0.0065;
}
*/

double getAltitude(double press, double temp)
{
  return ((pow((def_sea_press / press), 1 / 5.257) - 1.0) * (temp + 273.15)) / 0.0065;
}

double kalman_update(double measurement)
{
  static double x = lastAlt; //value
  static double p = 100; //estimation error covariance
  static double k = 0; //kalman gain

  // update the prediction value
  p = p + kalman_q;

  // update based on measurement
  k = p / (p + kalman_r);
  x = x + k * (measurement - x);
  p = (1 - k) * p;

  return x;
}


void setup()
{
	//
	Serial.begin(115200);
	Wire.begin();
	Baro.init(MS561101BA_ADDR_CSB_LOW);
        delay(400);

	//
	for( int i = 0; i < climbRateQueueCount; i++ )
		climbRateQueue[i] = 0;

	//
        Serial.print("Enter "); Serial.println(millis());
        while( Baro.getPressure(OSS) == 0);
        Serial.print("Leave "); Serial.println(millis());

	for( int i = 0; i < pressureCount; i++ )
        {
  		double p = Baro.getPressure(OSS);
  		double t = Baro.getTemperature(OSS);
  		Serial.print(" p = "); Serial.println(p);
  		savePressure(p);
        }
		
	double pressure = getAveragePressure();
	lastAlt = getAltitude(pressure, Baro.getTemperature(OSS));

	lastMillis = millis();
}

void loop()
{
	double pres = Baro.getPressure(OSS);
	double temp = Baro.getTemperature(OSS);

	rawAlt = getAltitude(pres, temp);
	lastAlt = kalman_update(rawAlt);

	savePressure(pres);
	saveClimbRate(lastAlt);

	if( (lastMillis + 200) <= millis() )
	{
		lastMillis = millis();

//		Serial.print("Pressure : "); Serial.println(pres);
//		Serial.print("Temperature : "); Serial.println(temp);
//		Serial.print("Raw Altitude : "); Serial.println(rawAlt);
//		Serial.print("Filtered Altitude : "); Serial.println(lastAlt);
		Serial.print("Climb Rate : "); Serial.println(getClimbRate());
	}
}

